#include <unistd.h>
#include <string.h>
#include <libplayercore/playercore.h>

#define DEFAULT_MIN_AREA 0
#define DEFAULT_MAX_AREA 40000
#define DEFAULT_MIN_DIST 0
#define DEFAULT_MAX_DIST 0
#define MAX_BLOBS 10

#define PI 3.141592654f
class BFPosition : public Driver
{

	public:
	
		// Constructor
   		BFPosition(ConfigFile* cf, int section);
		virtual int Setup();
		virtual int Shutdown();
		virtual int ProcessMessage(MessageQueue* resp_queue,player_msghdr *hdr,void * data);

	private:
		virtual void Main();
		
		void findBlobsCouple(int BColor,int RColor);
		void TransformPoint(int x, int y, double *X, double *Y);
		bool correctLensDist(double u, double v,double *X, double *Y);
		
		int intTest;
		
		//colors of blobs
		uint32_t baseColor;
		uint32_t robotColor;
		//min and max area of blobs
		uint32_t minArea;
		uint32_t maxArea;
		//min and max dist between blobs
		int minDist;
		int maxDist;
		
		//address and device of Blobfinder interface
		player_devaddr_t blobfinderAddr;
		Device* blobfinderDev;
		
		//buffers for desired blobs 
		int BCBlobID[MAX_BLOBS];
		int RCBlobID[MAX_BLOBS];
		
		//transformation matrix, used to proyect point in a defined plane
		double transMatrix[9];
		
		//parameters for correction of lens distortion
		double radialDistParams[2];
		
		//matrix of intrinsic params
		double intrinsicParams[9];
		
		//flag indicating if a proyection will take place
		bool proyEnabled;
		bool radialCorrEnabled;
		bool intrParamEn;
		//last data of blobs 
		player_blobfinder_data_t* blobs_data;
		
		player_position2d_data_t pos_data;
		
		
		
		
};


